%% LQR
clc

% t = 0:0.001:10;
% theta40 = theta40(1:10001)';
% theta63 = theta63(1:10001)';
% theta120 = theta120(1:10001)';
% xlast40 = xlast40(1:10001)';
% xlast63 = xlast63(1:10001)';
% xlast120 = xlast120(1:10001)';

%Save = figure('Color',[1 1 1]);
figure(1)
plot(t,xlast40,'b-',t,theta40,'b--',...
    t,xlast63,'g-',t,theta63,'g--',...
    t,xlast120,'r-',t,theta120,'r--')
angle = refline([0 -0.079]);
overshoot = refline([0 0.84]);
set(angle,'Color','c','LineStyle','--');
set(overshoot,'Color','c','LineStyle','--');
grid;
axis([0 8 -0.1 0.9]);
xlabel('Tid [s]');
ylabel('Hastighed [m/s] / Vinkel [rad]');
legend('Hastighed (0,4 m)','Vinkel (0,4 m)','Hastighed (0,638 m)','Vinkel (0,638 m)','Hastighed (1,2 m)','Vinkel (1,2 m)','Krav');
set(legend,'Position',[0.63,0.57,0.1,0.1]);
set(gcf,'PaperUnits','inches','PaperPosition',[0 +2 10 5]);
%print(Save,'-depsc','lqr');

%% Real
clc

% t = 0:0.001:10;
% theta40 = theta40(1:10001)';
% theta63 = theta63(1:10001)';
% theta120 = theta120(1:10001)';
% xlast40 = xlast40(1:10001)';
% xlast63 = xlast63(1:10001)';
% xlast120 = xlast120(1:10001)';

%Save = figure('Color',[1 1 1]);
figure(2)
plot(t,xlast40,'b-',t,theta40,'b--',...
    t,xlast63,'g-',t,theta63,'g--',...
    t,xlast120,'r-',t,theta120,'r--')
angle = refline([0 -0.079]);
overshoot = refline([0 0.84]);
set(angle,'Color','c','LineStyle','--');
set(overshoot,'Color','c','LineStyle','--');
grid;
axis([0 8 -0.1 0.9]);
xlabel('Tid [s]');
ylabel('Hastighed [m/s] / Vinkel [rad]');
legend('Hastighed (0,4 m)','Vinkel (0,4 m)','Hastighed (0,638 m)','Vinkel (0,638 m)','Hastighed (1,2 m)','Vinkel (1,2 m)','Krav');
set(legend,'Position',[0.63,0.57,0.1,0.1]);
set(gcf,'PaperUnits','inches','PaperPosition',[0 +2 10 5]);
%print(Save,'-depsc','real');

%% LQR vs Real
clc

% t = 0:0.001:10;
% theta120lqr = theta120lqr(1:10001)';
% theta120real = theta120real(1:10001)';
% xlast120lqr = xlast120lqr(1:10001)';
% xlast120real = xlast120real(1:10001)';
% ylast120lqr = ylast120lqr(1:10001)';
% ylast120real = ylast120real(1:10001)';

%Save = figure('Color',[1 1 1]);
figure(3)
plot(t,xlast120lqr,'b-',t,ylast120lqr,'b-.',t,theta120lqr,'b--',...
    t,xlast120real,'r-',t,ylast120real,'r-.',t,theta120real,'r--')
angle = refline([0 -0.079]);
overshoot = refline([0 0.84]);
settle = refline([0 0.76]);
set(angle,'Color','c','LineStyle','--');
set(overshoot,'Color','c','LineStyle','--');
set(settle,'Color','c','LineStyle','--');
grid;
axis([0 8 -0.1 0.9]);
xlabel('Tid [s]');
ylabel('Hastighed [m/s] / Vinkel [rad]');
legend('X-hastighed (LQR)','Y-hastighed (LQR)','Vinkel (LQR)','X-hastighed (Reele)','Y-hastighed (Reele)','Vinkel (Reele)','Krav');
set(legend,'Position',[0.63,0.57,0.1,0.1]);
set(gcf,'PaperUnits','inches','PaperPosition',[0 +2 10 5]);
%print(Save,'-depsc','lqrvsreal');
